#include "OneLayerRungeKutta.h"

#include <iostream>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

#include "Clock.h"

int llg (double t, const double y[], double f[], void *params)
{
  double *p=(double *)params;
  double alpha=p[0];
  double gamma=p[1];
  double beta1=p[2];
  double beta2=p[3];

  double heffx=p[4];
  double heffy=p[5];
  double heffz=p[6];

  double px=p[7];
  double py=p[8];
  double pz=p[9];

  double mx=y[0];
  double my=y[1];
  double mz=y[2];  

  f[0]=-gamma*(my*heffz-mz*heffy)-alpha*((mx*mx-1)*heffx+mx*my*heffy+mx*mz*heffz)
    +beta1*(my*pz-mz*py)-beta2*((mx*mx-1)*px+mx*my*py+mx*mz*pz);
  f[1]=-gamma*(mz*heffx-mx*heffz)-alpha*((my*my-1)*heffy+mz*my*heffz+mx*my*heffx)
    +beta1*(mz*px-mx*pz)-beta2*((my*my-1)*py+mz*my*pz+mx*my*px);
  f[2]=-gamma*(mx*heffy-my*heffx)-alpha*((mz*mz-1)*heffz+mz*mx*heffx+mz*my*heffy)
    +beta1*(mx*py-my*px)-beta2*((mz*mz-1)*pz+mz*mx*px+mz*my*py);  

  return GSL_SUCCESS;
}

int jac (double t, const double y[], double *dfdy, double dfdt[], void *params)
{
  double *p=(double *)params;
  double alpha=p[0];
  double gamma=p[1];
  double beta1=p[2];
  double beta2=p[3];

  double heffx=p[4];
  double heffy=p[5];
  double heffz=p[6];

  double px=p[7];
  double py=p[8];
  double pz=p[9];

  double mx=y[0];
  double my=y[1];
  double mz=y[2];

  gsl_matrix_view dfdy_mat 
    = gsl_matrix_view_array (dfdy, 3, 3);
  gsl_matrix * m = &dfdy_mat.matrix; 
  gsl_matrix_set(m,0,0,-alpha*(2*mx*heffx+my*heffy+mz*heffz)-beta2*(2*mx*px+my*py+mz*pz));
  gsl_matrix_set(m,0,1,-gamma*heffz-alpha*mx*heffy+beta1*pz-beta2*mx*py);
  gsl_matrix_set(m,0,2,gamma*heffy-alpha*mx*heffz-beta1*py-beta2*mx*pz);
  gsl_matrix_set(m,1,0,gamma*heffz-alpha*my*heffx-beta1*pz-beta2*my*px);
  gsl_matrix_set(m,1,1, -alpha*(2*my*heffy+mx*heffx+mz*heffz)-beta2*(2*my*py+mx*px+mz*pz));
  gsl_matrix_set(m,1,2,-gamma*heffx-alpha*my*heffz+beta1*px-beta2*my*pz);
  gsl_matrix_set(m,2,0,-gamma*heffy-alpha*mz*heffx+beta1*py-beta2*mz*px);
  gsl_matrix_set(m,2,1,gamma*heffx-alpha*mz*heffy-beta1*px-beta2*mz*py);
  gsl_matrix_set(m,2,2,-alpha*(2*mz*heffz+mx*heffx+my*heffy)-beta2*(2*mz*pz+my*py+mx*px));
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  dfdt[2] = 0.0;
  return GSL_SUCCESS;
}

double* rungekutta(Torque torque,Layer pol,Vector3 mag,Clock clock)
{

  double *arr=new double[3];

  const gsl_odeiv_step_type * T 
    = gsl_odeiv_step_rkck;
  
  gsl_odeiv_step * s 
    = gsl_odeiv_step_alloc (T, 3);
  gsl_odeiv_control * c 
    = gsl_odeiv_control_y_new (1E-5, 1);//first is max absolute error second is relative
  gsl_odeiv_evolve * e 
    = gsl_odeiv_evolve_alloc (3);
  
  double alpha=torque.getAlpha();
  double gamma=torque.getGamma();
  double beta1=torque.getBeta1();
  double beta2=torque.getBeta2();

  double px=pol.getMag().getX();
  double py=pol.getMag().getY();
  double pz=pol.getMag().getZ();

  double heffx=torque.getHeff().getX();
  double heffy=torque.getHeff().getY();
  double heffz=torque.getHeff().getZ();
       
  double params[9];

  gsl_odeiv_system system = {llg, jac, 3, params};

  params[0]=alpha;
  params[1]=gamma;
  params[2]=beta1;
  params[3]=beta2;
  params[4]=heffx;
  params[5]=heffy;
  params[6]=heffz;
  params[7]=px;
  params[8]=py;
  params[9]=pz;
     
  double t = clock.getTimeStamp(), t1 = clock.getTimeStamp()+clock.getTimeStep(); //t=initial time t1=final time
  double h = 1E-10; //initial stepsize

  arr[0]=mag.getX(); 
  arr[1]=mag.getY();
  arr[2]=mag.getZ(); 

  while (t < t1)
    {
      int status = gsl_odeiv_evolve_apply (e,c,s,&system,&t,t1,&h,arr);
      
      if (status != GSL_SUCCESS)
	break;
      
      mag.setX(arr[0]);
      mag.setY(arr[1]);
      mag.setZ(arr[2]);

    }
 
  gsl_odeiv_evolve_free (e);
  gsl_odeiv_control_free (c);
  gsl_odeiv_step_free (s);
 
  return arr;
}
